/*
 * 	MotorControl.c
 *
 *	Description: Helper functions to control the four 
 *	platform steppers
 *
 *  Created on: Nov 5, 2012
 *      Author: banfield
 */

 
/*****************************************************
 * Include Statements
 ****************************************************/
#include "hw_types.h"
#include "gpio.h"
#include <stdlib.h>
#include <stdio.h>
#include "inc/lm3s2965.h"
#include "MotorControl.h"
#include "interrupt.h"
#include "sysctl.h"
#include "hw_memmap.h"
#include "hw_types.h"
#include "hw_ints.h"


/*****************************************************
 * Define Statements
 ****************************************************/
#define MOTOR_PORT GPIO_PORTG_DATA_R	// port used for the enable and direction lines  
#define STEP_PORT GPIO_PORTD_DATA_R		// port used for the step output

#define DIR_PIN 0b1000	// pin for the direction output
#define STEP_PIN 0b10	// pin for the step output 
#define	STEP_TRANS_DIST 0.007854	//translational distance from one 1.8 degree step (based on spur pitch diameter)
#define STEP_ROT_DIST	0.0777		//rotational increment per step (degrees)
#define	SLOP_MARGIN		1.2			//slop percentage (120%)
#define	STEP_DELAY 10000	// delay used to create step signals

// limit switch defines
// limit switch inputs are on PB0, PB1, and PB2 
#define LIMIT_SWITCH_PERIPH SYSCTL_PERIPH_GPIOB
#define LIMIT_SWITCH_INT INT_GPIOB
#define LIMIT_SWITCH_MASK 0b00000111
#define LIMIT_SWITCH_BASE GPIO_PORTB_BASE
#define X_LIMIT_SWITCH 0b00000001
#define Y_LIMIT_SWITCH 0b00000010
#define PHI_LIMIT_SWITCH 0b00000100


/*****************************************************
 * Module Level Variables
 ****************************************************/
static float currentX;		// current platform position in the direction parallel to the car
static float currentY;		// current platform position in the direction perpendicular to th car
static float currentPhi;	// current rotational position

// flags used when returning to the home position 
static char xHomeFlag;		
static char yHomeFlag;
static char phiHomeFlag;
static char homingX;
static char homingY;
static char homingPhi;
//driver board is hard-wired to full step mode


/*****************************************************
 * Function: InitMotors 
 *
 * Description: Initialize all necessary port, 
 * peripheral, and module level variables
 ****************************************************/
void InitMotors(void){
	unsigned long dummy;

    // Enable the GPIO ports that is used for motor control.
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);

    // Do a dummy read to insert a few cycles after enabling the peripheral.
    dummy = SYSCTL_RCGC2_R;

    //Enable the GPIO port that used for homing interrupts
	SysCtlPeripheralEnable(LIMIT_SWITCH_PERIPH);
	GPIOPinTypeGPIOInput(GPIO_PORTB_BASE, LIMIT_SWITCH_MASK); // why the hell is this not the same as ->  GPIODirModeSet(LIMIT_SWITCH_BASE, LIMIT_SWITCH_MASK, GPIO_DIR_MODE_IN);

	//Enable motor interrupts
	IntMasterEnable();
	//IntEnable(LIMIT_SWITCH_INT);
	//GPIOIntTypeSet(LIMIT_SWITCH_BASE, LIMIT_SWITCH_MASK,GPIO_FALLING_EDGE);
	//GPIOPinIntEnable(LIMIT_SWITCH_BASE,LIMIT_SWITCH_MASK);

    // Enable the GPIO pins for motor control.  Set the direction as output, and
    // enable the GPIO pin for digital function.
    GPIO_PORTG_AFSEL_R &= ~0b1111;
    GPIO_PORTG_DIR_R |= 0b1111;
    GPIO_PORTG_DEN_R |= 0b1111;

    GPIO_PORTD_AFSEL_R &= ~0b10;
    GPIO_PORTD_DIR_R |= 0b10;
    GPIO_PORTD_DEN_R |= 0b10;

	currentX = 0;
	currentY = 0;
	currentPhi = -90;

	homingX = 0;
	homingY = 0;
	homingPhi = 0;

	xHomeFlag = 0;
	yHomeFlag = 0;
	phiHomeFlag = 0;
}


/*****************************************************
 * Function: SetDirection 
 ****************************************************/
void SetDirection(int *steps){

	// the direction os set based on the sign of steps
	if(steps == HOME_DIR || *steps > 0){
		MOTOR_PORT |= DIR_PIN;
	}else{
		*steps = -*steps;
		MOTOR_PORT &= ~DIR_PIN;
	}
}


/*****************************************************
 * Function: SetMotor
 ****************************************************/
void SetMotor(char motor){
	// write motor selection to stellaris
	MOTOR_PORT &= 0b11111000;
	MOTOR_PORT |= motor;	//write the correct binary value to the motor port
}


/*****************************************************
 * Function: Step
 *
 * Description: Take a single step in the currently 
 * selected direction
 ****************************************************/
void Step(void){
	int i;

	STEP_PORT |= STEP_PIN;
	//this loop was bam bams idea, I said ASM no-ops (ichiban)
	for(i = 0 ; i < STEP_DELAY ; i++);

	STEP_PORT &= ~STEP_PIN;

	//this loop was bam bams idea, I said ASM no-ops (ichiban)
	for(i = 0 ; i < STEP_DELAY ; i++);
}


/*****************************************************
 * Function: SingleStepMotor
 *
 * Description: Set the direction and the motor 
 * and take a single step
 ****************************************************/
float SingleStepMotor(char motor, int direction){

	SetDirection(&direction);
	SetMotor(motor);
	Step();

	if(motor == X_MOTOR){
		currentX += (STEP_TRANS_DIST * direction);
		return currentX;
	}else if(motor == Y_MOTOR){
		currentY += (STEP_TRANS_DIST * direction);
		return currentY;
	}else{
		currentPhi += (STEP_ROT_DIST * direction);
		return currentPhi;
	}
}


/*****************************************************
 * Function: MoveMotor
 *
 * Description: move the specified motor to endPos. 
 * The appropriate current position must be given in 
 * order to calculate the correct number of steps.
 ****************************************************/
static void MoveMotor(char motor, float endPos, float *currentPos){
	int steps, i;

	if(motor == PHI_MOTOR){
		steps = (endPos - *currentPos)/STEP_ROT_DIST;
	}else{
		steps = (endPos - *currentPos)/STEP_TRANS_DIST;
	}

	SetMotor(motor);
	SetDirection(&steps);
	for(i = 0 ; i < steps ; i++){
		Step();
	}

	*currentPos = endPos;
}


/*****************************************************
 * Function: MoveToCoord
 *
 * Description: Move the platform to an (x, y, phi)
 * position  
 ****************************************************/
void MoveToCoord(float x, float y, float phi){
	
	printf("MotorControl: MoveToCoord x = %f, y = %f, phi = %f\n", x, y, phi);
	MoveMotor(X_MOTOR, x, &currentX);
	MoveMotor(Y_MOTOR, y, &currentY);
	MoveMotor(PHI_MOTOR, phi, &currentPhi);
}


/*****************************************************
 * Function: ResetMotor
 *
 * Description: Return the specified motor to it's 
 * home position
 ****************************************************/
static void ResetMotor(char motor, float *position, char *homeFlag){
	SetMotor(motor);
	SetDirection(HOME_DIR);

	while(!(*homeFlag)){
		Step();
	}

	*position = 0;
}


/*****************************************************
 * Function: ReturnHome
 *
 * Description: Return the platform to the home position
 ****************************************************/
void ReturnHome(void){
	// homing flags set while the platform moves 
	homingX = 1;
	homingY = 1;
	homingPhi = 1;

	puts("Returning home.");

	ResetMotor(X_MOTOR, &currentX, &xHomeFlag);
	ResetMotor(Y_MOTOR, &currentY, &yHomeFlag);
	ResetMotor(PHI_MOTOR, &currentPhi, &phiHomeFlag);

	homingX = 0;
	homingY = 0;
	homingPhi = 0;
}


/*****************************************************
 * Function: GetCurrentX
 ****************************************************/
float GetCurrentX(void){
	return currentX;
}


/*****************************************************
 * Function: GetCurrentY
 ****************************************************/
float GetCurrentY(void){
	return currentX;
}


/*****************************************************
 * Function: GetCurrentPhi
 ****************************************************/
float GetCurrentPhi(void){
	return currentPhi;
}


/*****************************************************
 * Function: GPIO_PORTB_Int_Handler
 *
 * Description: limit switch interrupt handler 
 ****************************************************/
void GPIO_PORTB_Int_Handler(void){

	unsigned long ulStatus;
	ulStatus = GPIOPinIntStatus(LIMIT_SWITCH_BASE, true);
	GPIOPinIntClear(LIMIT_SWITCH_BASE, ulStatus);
	switch(ulStatus){
			case X_LIMIT_SWITCH:
					//Stop Motor x

					if(homingX==1)//We were trying to home X
					{
							//Post an event
							xHomeFlag = 1;
					}
					else{//We got here by accident - stay put
							while(1);
					}
			break;
			case Y_LIMIT_SWITCH:
					//Stop Motor y
					if(homingY==1)//We were trying to home Y
					{
							//Post an event
							yHomeFlag = 1;
					}
					else{//We got here by accident - stay put
							while(1);
					}
			break;
			case PHI_LIMIT_SWITCH:
					//Stop Motor phi
					if(homingPhi==1)//We were trying to home Y
					{
							//Post an event
							phiHomeFlag = 1;
					}
					else{//We got here by accident - stay put
							while(1);
					}
			break;

	}

}

